package org.noote.libs.sensors;

import java.util.List;

import android.content.Context;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;

// http://www.oelerich.org/index.php/on-the-accelerometer-of-the-htc-desire/

public class LinearAccelerometerManager {

    private Sensor _sensorAccelerometer;
    private Sensor _sensorLinearAccelerometer;
    private Sensor _sensorMagneto;    
    private SensorManager _sensorManager;
    private LinearAccelerometerListener _listener;
 
    /** indicates whether or not Accelerometer Sensor is supported */
    private boolean _bSupported = false;
    /** indicates whether or not Accelerometer Sensor is running */
    private boolean _bRunning = false;
 
	private static final int matrix_size = 9;
    float[] inR = new float[matrix_size];
    float[] I = new float[matrix_size];
    float[] values = new float[3];
    float[] accels = new float[3];
    float[] laccels = new float[3];
    float[] mags = new float[3];
    float[] loc = new float[]{0,0,0};
    float[] veloc = new float[]{0,0,0};
    float distance = 0;
    long time = 0;
    long startTime = 0;
    boolean measure = false;
    int counter = 0;
    float correction=0;
    
	/**
     * Returns true if the manager is listening to orientation changes
     */
    public boolean isListening() {
        return _bRunning;
    }
 
    /**
     * Unregisters listeners
     */
    public void stopListening() {
    	_bRunning = false;
        try {
            if (_sensorManager != null && _sensorEventListener != null) {
                _sensorManager.unregisterListener(_sensorEventListener);
            }
        } catch (Exception e) {}
    }
 
    /**
     * Returns true if at least one Accelerometer _sensor is available
     */
    public boolean isSupported(Context ctx) {
        if (ctx != null) {
        	_sensorManager = (SensorManager) ctx.getSystemService(Context.SENSOR_SERVICE);
            List<Sensor> _sensorsA = _sensorManager.getSensorList(Sensor.TYPE_ACCELEROMETER);
            List<Sensor> _sensorsL = _sensorManager.getSensorList(Sensor.TYPE_LINEAR_ACCELERATION);
            List<Sensor> _sensorsM = _sensorManager.getSensorList(Sensor.TYPE_MAGNETIC_FIELD);
            _bSupported = (_sensorsA.size() > 0) && (_sensorsL.size() > 0) && (_sensorsM.size() > 0);
        } else {
        	_bSupported = false;
        }
        return _bSupported;
    }
 
    /**
     * Registers a _listener and start listening
     * @param accelerometerListener
     *             callback for accelerometer events
     */
    public void startListening(Context ctx, LinearAccelerometerListener listener)
    {
        _sensorManager = (SensorManager) ctx.getSystemService(Context.SENSOR_SERVICE);
        List<Sensor> _sensorsA = _sensorManager.getSensorList(Sensor.TYPE_ACCELEROMETER);
        if (_sensorsA.size() > 0) {
        	_sensorAccelerometer = _sensorsA.get(0);
        }
        List<Sensor> _sensorsL = _sensorManager.getSensorList(Sensor.TYPE_LINEAR_ACCELERATION);
        if (_sensorsL.size() > 0) {
        	_sensorLinearAccelerometer = _sensorsL.get(0);
        }
        List<Sensor> _sensorsM = _sensorManager.getSensorList(Sensor.TYPE_MAGNETIC_FIELD);
        if (_sensorsM.size() > 0) {
        	_sensorMagneto = _sensorsM.get(0);
        }
        _bRunning = _sensorManager.registerListener(_sensorEventListener, _sensorAccelerometer, SensorManager.SENSOR_DELAY_FASTEST);
        _bRunning = _sensorManager.registerListener(_sensorEventListener, _sensorLinearAccelerometer, SensorManager.SENSOR_DELAY_FASTEST);
        _bRunning = _sensorManager.registerListener(_sensorEventListener, _sensorMagneto, SensorManager.SENSOR_DELAY_FASTEST);
        
        _listener = listener;        
    }
 
    public void startMeasure() {
        startTime = time = System.currentTimeMillis();
        loc = new float[]{0,0,0};
        veloc = new float[]{0,0,0};
        distance = 0;
        measure = true;
    }
   
    public void stopMeasure() {
        measure = false;
    }
    
    /**
     * The _listener that listen to events from the accelerometer _listener
     */
    private SensorEventListener _sensorEventListener = 
        new SensorEventListener() {
 
        @Override
		public void onAccuracyChanged(Sensor sensor, int accuracy) {}
 
        @Override
		public void onSensorChanged(SensorEvent event) {
            
        	if (event.accuracy == SensorManager.SENSOR_STATUS_UNRELIABLE)
                return;
        	
        	switch (event.sensor.getType()) {
            case Sensor.TYPE_MAGNETIC_FIELD:
                mags = event.values.clone();
                _listener.onOrientationChanged(mags[SensorManager.DATA_X], mags[SensorManager.DATA_Y], mags[SensorManager.DATA_Z]);
                break;
            case Sensor.TYPE_LINEAR_ACCELERATION:
                laccels = event.values.clone();
                break;
            case Sensor.TYPE_ACCELEROMETER:
                accels = event.values.clone();
                _listener.onAccelerationChanged(accels[SensorManager.DATA_X], accels[SensorManager.DATA_Y], accels[SensorManager.DATA_Z]);
                break;
        	}        	
        	
        	if (mags != null && accels != null && laccels != null && measure) {

                SensorManager.getRotationMatrix(inR, I, accels, mags);
                values = convertVector(laccels, inR);
               
                long now = System.currentTimeMillis();
                float dt = (float)((now-time)/1000.0);
               
                float dx = veloc[0]*dt + 0.5f*values[0]*dt*dt;
                float dy = veloc[1]*dt + 0.5f*values[1]*dt*dt;
                float dz = veloc[2]*dt + 0.5f*values[2]*dt*dt;

                loc[0] += dx;
                loc[1] += dy;
                loc[2] += dz;
                veloc[0] += values[0]*dt;
                veloc[1] += values[1]*dt;
                veloc[2] += values[2]*dt;
                distance += Math.sqrt(Math.pow(dx, 2)+Math.pow(dy, 2)+Math.pow(dz, 2));
                
                _listener.onVelocityChanged(veloc[0], veloc[1], veloc[2]);
                _listener.onPositionChanged(loc[0], loc[1], loc[2]);
               
                time = now;
                counter++;
            } 
        }
        
        private float[] convertVector(float[] vec, float[] matrix) {
            float[] result = new float[3];
           
            result[0] = matrix[0]*vec[0]+matrix[1]*vec[1]+matrix[2]*vec[2];
            result[1] = matrix[3]*vec[0]+matrix[4]*vec[1]+matrix[5]*vec[2];
            result[2] = matrix[6]*vec[0]+matrix[7]*vec[1]+matrix[8]*vec[2];
           
            return result;
        }        
    };
 }